#include <string.h>
#include <stdio.h>
#include "command.h"

//put this code into the name space robot
namespace robot {

ICommand::ICommand() {
}

ICommand::~ICommand() {
}

Command::Command(int id, int angle, int times) :
		m_ID(id), m_angle(angle), m_times(times) {
}

Command::~Command() {
}

void Command::ToString(char *buf) {
	int ptr = 0;
	buf[ptr++] = '#';
	itoa(m_ID, &buf[ptr], 10);
	ptr = strlen(buf);
	buf[ptr++] = 'P';
	itoa(static_cast<int>(m_angle * 200.0 / 18.0 + 500), &buf[ptr], 10);
	ptr = strlen(buf);
	buf[ptr++] = 'T';
	itoa(m_times, &buf[ptr], 10);
}

CommandSet::CommandSet() {
}

CommandSet::~CommandSet() {
}

void CommandSet::ToString(char *buf) {
	int ptr = 0;
	int i = 0;
	for (std::vector<int>::iterator it = m_ID_Set.begin(); it != m_ID_Set.end();
			++it, ++i) {
		buf[ptr++] = '#';
		itoa(m_ID_Set[i], &buf[ptr], 10);
		ptr = strlen(buf);
		buf[ptr++] = 'P';
		itoa(static_cast<int>(m_Angle_Set[i] * 200.0 / 18.0 + 500), &buf[ptr],
				10);
		ptr = strlen(buf);
	}
	buf[ptr++] = 'T';
	std::vector<int>::iterator begin_times = m_Times_Set.begin();
	itoa(*begin_times, &buf[ptr], 10);
}

void CommandSet::AddCommand(const Command &command) {
	m_ID_Set.push_back(command.GetID());
	m_Angle_Set.push_back(command.GetAngle());
	m_Times_Set.push_back(command.GetTimes());
}

CommandGroup::CommandGroup(const std::vector<int> &group_id, int times) :
		m_group_id(group_id), m_times(times) {
}

CommandGroup::~CommandGroup() {
}

void CommandGroup::ToString(char *buf) {
	int ptr = 0;
	for (std::vector<int>::iterator it = m_group_id.begin();
			it != m_group_id.end(); ++it) {
		buf[ptr++] = '#';
		itoa(static_cast<int>((*it) * 200.0 / 18.0 + 500), &buf[ptr], 10);
		ptr = strlen(buf);
		buf[ptr++] = 'G';
	}

	buf[ptr++] = 'C';
	itoa(m_times, &buf[ptr], 10);
}

void itoa(unsigned long val, char *buf, unsigned radix) {
	char *p;
	char *firstdig;
	char temp;
	unsigned digval;

	p = buf;
	firstdig = p;

	do {
		digval = (unsigned) (val % radix);
		val /= radix;
		if (digval > 9) {
			*p++ = (char) (digval - 10 + 'a ');
		} else {
			*p++ = (char) (digval + '0 ');
		}
	} while (val > 0);

	*p-- = '\0 ';

	do {
		temp = *p;
		*p = *firstdig;
		*firstdig = temp;
		--p;
		++firstdig;
	} while (firstdig < p);
}
}

